import cv2
import numpy as np
import math
import time
# import imutils
# from  imutils.video import VideoStream, videostream
# from pyzbar import pyzbar
# from PIL import Image

import argparse
import datetime
# from PIL import Image,ImageEnhance
import serial as ser
import binascii
import pyrealsense2 as rs
from color_dist import *
import random

barcode_sta=0

#########################  项目日志
##### 9.24 识别二维码 完成    
#####               尚未与电控协定发送方式？  数字的对应顺序
#####      识别物料（与电控交流后发现可以跳过这一步）
#####      识别靶心   算法实现效果：摄像头保持开启，按二维码读取顺序循环识别颜色
#####                与电控协商方案：到达制定位置后电控开始接受信息   
#####                算法实现设想：识别到当前所需颜色后向电控发送数据  （尚未协定需发送数据的内容）
#####      识别旋转盘上的颜色及位置：同上





#串口

def port_open(): #串口打开函数

    if(ser.isOpen()==False):
        ser.open()

    if(ser.isOpen()):
        print("串口打开成功")
    else:
        print("串口打开失败")

def port_close(): #串口关闭函数

    if(ser.isOpen()):
        ser.close()

    if(ser.isOpen()==False):
        print("串口关闭成功")
    else:
        print("串口关闭失败")


def show_colorizer_depth_img():
    '''
    show colorized depth img
    '''
    global depth_frame, color_image
    colorizer = rs.colorizer()
    hole_filling = rs.hole_filling_filter()
    filled_depth = hole_filling.process(depth_frame)
    colorized_depth = np.asanyarray(colorizer.colorize(filled_depth).get_data())
    cv2.imshow('filled depth',colorized_depth)


def object_color_detect(color):
    '''
    detect the color object
    '''
    global depth_frame, color_image
    hsvFrame = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
    #空间转换函数 （color_image--需要转换的图片，格式）
    # HSV
    color_lower = np.array(color_dist[color]["Lower"], np.uint8) 
    color_upper = np.array(color_dist[color]["Upper"], np.uint8) 
    color_mask = cv2.inRange(hsvFrame, color_lower, color_upper) 
	
    color_mask = cv2.medianBlur(color_mask, 9)  # 中值滤波

    # kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))  # 矩形结构
    # kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))  # 椭圆结构
    kernel = cv2.getStructuringElement(cv2.MORPH_CROSS, (5, 5))  # 十字形结构

    #kernerl 进行操作的内核，矩阵
    color_mask = cv2.dilate(color_mask, kernel)  # 膨胀
    kernel = np.ones((10, 10), np.uint8)
    color_mask = cv2.erode(color_mask, kernel)  # 腐蚀
	
    res = cv2.bitwise_and(color_image, color_image, mask = color_mask)  #位运算

    #cv2.imshow("Color Detection res in Real-Time", res)

	# Creating contour to track red color 
    contours, hierarchy = cv2.findContours(color_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    #contour 矩阵的登高线图
    try:

        # 最小外接正矩形
        c = max(contours, key=cv2.contourArea)
        left_x, left_y, width, height = cv2.boundingRect(c)

        bound_rect = np.array([[[left_x, left_y]], [[left_x + width, left_y]],
                               [[left_x + width, left_y + height]], [[left_x, left_y+height]]])
        box_list = bound_rect.tolist()
        cv2.drawContours(color_image, [bound_rect], -1, (255, 255, 255), 2)

    except ValueError:
        box_list = []

    #try:  except  可能产生异常的代码块+处理异常的代码块        
    
    return box_list



def object_distance_measure(bbox_list):
    global depth_frame, color_image
    if bbox_list != []:
        # print(type(bbox_list))
        print(bbox_list)
        #    1 >>>>>>> 2
        #              |
        #              |
        #              |
        #    4 <<<<<<< 3

        left_x=bbox_list[0][0][0]
        left_y=bbox_list[0][0][1]
        left = bbox_list[0][0][0]
        right = bbox_list[1][0][0]
        top = bbox_list[1][0][1]
        bottom = bbox_list[3][0][1]
        width = right - left
        height = bottom - top

    
        # 测距的区域
        roi_lx = int(left + width/4)
        roi_rx = int(right - width/4)
        roi_ty = int(top + height/4)
        roi_by = int(bottom - height/4)
        # print(roi_lx, roi_rx, roi_ty, roi_by)
        color_image= cv2.rectangle(color_image, (roi_lx, roi_ty), (roi_rx, roi_by), (255, 255, 0),3) 

        center_x = int(left_x + width/2)
        center_y = int(left_y + height/2)

        # print("center_x:",center_x)
        # print("center_y:",center_y)
        
        #转换坐标系后的中心坐标
        # global centernew_x,centernew_y

        # centernew_x=center_x-320
        # centernew_y=center_y-240

        cv2.circle(color_image, (center_x, center_y), 5, (0,0,255), 0)

        depth_points = []
        depth_means = []
        
        # 获取目标框内的物体距离，并进行均值滤波
        for j in range(50):
            rand_x = random.randint(roi_lx, roi_rx)
            rand_y = random.randint(roi_ty, roi_by)
            depth_point = round(depth_frame.get_distance(rand_x, rand_y)*100, 2)
            if depth_point != 0:
                depth_points.append(depth_point)
        depth_object = np.mean(depth_points)
        if depth_object >= 30:
            print("The camera is facing an object mean ", int(depth_object), " cm away.")
        else:
            print("The camera is facing an object mean <30 cm away.")


def barcode_recog(color_image):

        global barcode_res
        
    # qrcode_image = cv2.imread(color_image)
        qrCodeDetector = cv2.QRCodeDetector()
        global barcode_sta
        data, bbox, straight_qrcode = qrCodeDetector.detectAndDecode(color_image)
        
        if(data):
            barcode_sta=1
            print(data) 
            #写入串口数据
            ser.write(data)

        else:
            print("未能识别二维码")
        
        return data

            #ser.write(data)

      
   
def color_detect_order(barcode_res):
  
    for s in barcode_res :

        #识别结果传给电控！！！！！！！！！！！！！！！！！！！！！！！！

        if(s=="1"):  

            print("识别红色：")  #  1对应红色

            bbox_list = object_color_detect(color="red")

            object_distance_measure(bbox_list)

        if(s=="2"):    

            print("识别绿色：")  #  2对应绿色

            bbox_list = object_color_detect(color="green")

            object_distance_measure(bbox_list)

        if(s=="3"):

            print("识别蓝色：")  #  3对应蓝色

            bbox_list = object_color_detect(color="blue")

            object_distance_measure(bbox_list)
        
   

        
        

if __name__ == "__main__":
    global depth_frame, color_image
    
    # Configure depth and color streams
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
    # Start streaming
    pipeline.start(config)
 
    try:
        while True:
            start = time.time()
            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()
             
            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            if not depth_frame or not color_frame:
                continue

            # Convert images to numpy arrays
            color_image = np.asanyarray(color_frame.get_data())

            # task program

            #识别二维码
            if(barcode_sta==0):
                barcode_res=barcode_recog(color_image)
            #循环，一共进行两次流程
            for i in range(2):
            #识别物料
                color_detect_order(barcode_res)
            #识别靶心&&识别圆盘

            # show image
            cv2.imshow("color_image", color_image)

            #输出识别时间
            # print("FPS:", 1/(time.time()-start), "/s")

            # Press esc or 'q' to close the image window
            key = cv2.waitKey(1)
            if key & 0xFF == ord('q') or key == 27:
                cv2.destroyAllWindows()
                break
    finally:
        # Stop streaming
        pipeline.stop()


# qrcode_filename = "D:/test.jpg"
# qrcode_image = cv2.imread(qrcode_filename)
# qrCodeDetector = cv2.QRCodeDetector()
# data, bbox, straight_qrcode = qrCodeDetector.detectAndDecode(qrcode_image)
# print(data) 